/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef VIRTUALLINECONTROL_H
#define VIRTUALLINECONTROL_H
#include <QQueue>
#include <qvector.h>
#include "MacroDefine/GeneralDefine.h"
#include "frames.hpp"
#include "trajectoryudefine.h"
#include "vehicledefine.h"


class MotionMessage;
class CoordinateTrajectory;
class AxisObject;
class MagneticTrack ;
class LaserSafetyDetect;
class VirtualLineBias;
class RobotIo;
using namespace KDL;
/**
 * @brief The VirtualLineControl class 虚拟轨迹跟踪类
 */
class VirtualLineControl
{
public:
    VirtualLineControl(QString configFileIn,int robotIdIn,RobotCommonParameter * roboParameterIn,
                       MotionMessage *motionMessageIn,MagneticTrack* maganeticTrackIn,
                       E_VEHICLE_TYPE vehicleTypeIn,RobotIo *ioControllerIn);

public:
    int controlTypeSwitch(int controlTypeIn);
    int getVehicleTrajectoryControlParameter(struct VehicleTrajectoryControlParameter &infoOut);
    int setVehicleTrajectoryControlParameter(struct VehicleTrajectoryControlParameter infoIn,int &returnFlag);
    int getAgvBorderInfo(struct VehicleBorderInfo &infoOut);
    int setAgvBorderRectInfo(struct BorderRectInfo rectInfoIn,int &returnFlag);
    int getAgvSheftInfo(int sheftIndex, struct BorderRectInfo &infoOut);
    int setAgvSheftRectInfo(int sheftIndex, struct BorderRectInfo rectInfoIn,int &returnFlag);

    double getBarrierProtectRatioOfMoving();
    int setPathMovingInfo_virtualLineControl(PathMovingInfo pathInfoIn);
    int getPathMovingInfo_virtualLineControl(PathMovingInfo &infoOut);
    /**
     * @brief getTrajectoryPoint_steeringAngle
     * @return degree 转向角度
     */
    int getTrajectoryPoint_steeringAngle(QVector<double> &steeringAngleOut);
    E_BIAS_TYPE getCalculateBiasType();
    int getCurrentPathInformation(VehiclePosition &startPositionOut, QVector<VehiclePosition> &targetPositionOut, int &pointIndexOut, E_NAV_COMMAND &currentPathTypeOut,
                           double &currentPathParameter1Out, double &currentPathParameter2Out);
    E_BARRIER_RUN_MODE getBarrierRunMode();
    int setBarrierRunMode(E_BARRIER_RUN_MODE modeIn);
    int setLaserLose(bool isLose, int positionType);//会导致暂停无效，慎用
    int setDebugFlag(int flagIn);
    int setAgvPathFollowStrategy(E_PATH_CONTROL_STRATEGY stragetyIn);
    int getBarriorDetectMode(E_BARRIER_TETECT_MODE &modeOut);
    int getSafeCollisionInfo(int sensorType, CollisionPara &infoOut);
    int setSafeCollisionInfo(int sensorType, CollisionPara infoIn,int &returnFlag);
    int setCurrentVehicleBorderId(int idIn);
    int getCurrentVehicleBorderId();
    int setVirtualLineEnableKpChange(bool isEnable);
    int getSafeSpeedRatio(double &ratioOut);
    int setTargetAllowOffset(float allowLineOffsetIn,float allowAngleOffsetIn);
    int getTimeConsumed_pointCloudDetect(double &minConsumedOut,double &currentConsumedOut,double &maxConsumedOut);
    int setVirtualLinePid(double kpDistanceErrorIn, double kpAngleDistanceIn,
                          double kpOnlyRotationIn, double kpDistanceError_yIn);
    int getTargetCommandVehiclePosition(VehiclePosition &targetOut, int &pointIndexOut);
    int getCarBorder(std::vector<VehiclePosition> &carBorderOut);
    int getCarBorderOffset(std::vector<VehiclePosition> &carBorderOffsetOut);
    int setPointCloudData(const QVector<PointCloudData_3D> &pointDataIn);
    int setVehicleBorderOffset(double front_offsetIn,double back_offsetIn,double left_offsetIn,double right_offsetIn);
    int setUltrasonicSensorCheckEnable(bool isOn);
    int getUltrasonicSensorSafeStatus(std::vector<double> &safeStatusOut,bool &isCheckEnableOut);
    int getPointCloudSafeStatus(std::vector<std::string > &sensorNameOut,std::vector<double> &sensorSafeRatioOut,
                                bool &isCheckEnableOut);
    int setPointCloudCheckEnable(bool isOn);
    int setAcceleration(double accelerationIn);
    int setAcceleration_rotate(double accelerationIn);
    int setServoStatus(bool isOn);
    int startRun();
    int cease();
    int quickStop();
    int stop();
    int resetAll();//清除全部
    int resetAll_exceptStatus();
    int addTrajectoryCommand(QVector<NavParameter> commandVectorIn);//添加一条轨迹命令
    int processLoopRealTime();
    int processLoopCommon();
    float getRunningNumber();
    int setRunningNumber_virtualLineControl(float numberIn);
    int getLineTimeDuration();
    int getLineRemainingTime();

    int getBias(BiasControlInfo &controlInfoOut);
    int getFollowOffset(double &currentLineOffsetOut, double &currentAngleOffsetOut);
    int setPositionFeedback(VehiclePosition vehiclePositionIn , VehicleVelocity vehicleVelocityIn,
                            QVector<double> steeringPositionIn);
    int setVehicleVelCommand(VehicleVelocity currentVehicleVelocityIn);
    int getVirtuleLineDebugInfo(VirtualLineDebugInfo& infoOut);
    int getVehiclePositionCommand(VehiclePosition &vehiclePositionOut);

    int setControlSpeedRatio(double controlSpeedRatioIn, double time);
    int setAdjustSpeedRatio(double adjustSpeedRatioIn, double time);
    int setSafeSpeedRatio(double safeSpeedRatioIn, double time);
    int sensorProtectLoop(bool isDetectLengthVariable, double &speedRatioOut);
    int calculateSensorProtect(bool isDetectLengthVariable, double &speedRatioOut);
    int isTrajectoryEmpty();//有ｂｕｇ ,行号表示.9了，但是这个函数返回非空，有一点时间差。
    bool isLineFinished(double currentLineTimeDuration);//修复行号逻辑。

private:
    int setVehicleDirection(bool isPositiveIn);
    double calculateSlowDownTime();
    int setTimeAxisScale(double ratio, double timeDuration);
    int synchronizeMotorFeedbackToCalculatePosition();
    int setBasicStatus(E_VIRTUAL_LINE_CONTROL_STATUS statusIn);
    int isAllVelProfileFinished();

    int isTargetOffsetCheckOk();
    bool isInFineMoving();
    int updateTrajTime();
    int getCommandQueueSize();
    int calculateVehicleFollowOffset(VehiclePosition vehiclePositionCommand);
    int stopOutput();


    int resetTrajTime();
    int moveOneStep();
    int timeAxisMoveOneStep();

    bool isServoOn();
    int addTrajTime(int lineNumber, int stationIdIn, int pathIdIn, double timeDuration);//调用者做锁保护
    int initialVirtualAxis();
    int processNavLineInstruction(NavParameter moveParameter);
    int processNavLineContinusInstruction(QVector<NavParameter> moveParameterArry, int isLastPointMove);
    int processNavSplineInstruction(NavParameter moveParameter);
    int processNavSplineContinusInstruction(QVector<NavParameter> moveParameterArry);
    int processNavSplineContinusInstructionWithControlPoints(QVector<NavParameter> moveParameterArry);
    int processNavRotateInstruction(NavParameter moveParameter);
    int printInformation(NavParameter moveParameter, QString title);
    int printInformation(QVector<NavParameter> moveParameterArry, QString title);
    int getCommandFromQueue(NavParameter & moveParameter);
    int calculateTrajectoryPath();//根据轨迹命令队列，计算轨迹。需要周期循环调用。
    void addMsg(int messageLevel, std::string componentName, std::string messageType
                , int messageCode, int robotIdIn, double parameter1=0, double parameter2=0,
                int parameter3=0, int parameter4=0, QString coment="");

    int initialTimeAxis();//初始时间轴
    int getTimeAxisStatus();
    int readConfigFile(QString configFilePathIn, QString &readComent);

    int ultrasonicSensorProtectLoop(VehicleVelocity vehicleVelocityIn, double &safeSpeedRatioOut);
    int initialPointCloudSafeDetect();
    int checkFollowOffset(double deltaDistanceIn, double deltaAngleIn);
    int dpointToVehiclePosition(DPoint dpointIn,VehiclePosition &positionOut);

private:
    E_VIRTUAL_LINE_CONTROL_STATUS basicStatus;
    pthread_mutex_t navMutex;
    pthread_mutexattr_t navAttr;
    QQueue<NavParameter> commandQueue;
    QQueue<int> lineQueue;
    QQueue<int> stationIdQueue;
    QQueue<int> pathIdQueue;
    PathMovingInfo pathMovingInfo;
    QQueue<double> timeDurationQueue;
    double currentLineTimeDuration;
    double currentLineRemainingTime;
    volatile float currentLine; //0.2=算法已取点, 0.5=正在运行，0.9=(已经到目标位置),  ,下一行+0.2(避免精度丢失,不用0.1)
    bool isCurrentLineFinished;
    VehiclePosition currentVehiclePosition;
    VehiclePosition currentVehiclePosition_old;
    VehicleVelocity currentVehicleVelocity;//命令速度，仅仅用于激光壁障计算方向。
    double steeringPositionFeedback[D_STEERING_MAX_COUNT];
    VirtualLineDebugInfo motionDebugInfo;

    MotionMessage * motionMessage;//need todo motionMessage is NULL
    CoordinateTrajectory *coordinateTrajectory;
    AxisObject* virtualAxis;//速度比例轴
    double deltaTime;


    double lineTimeCount;
    double lineTimeCountOld;
    int exceedTimeCount;
    bool isTrajectoryContiousRun;
    float targetWaitTime; // unit: s
    int robotId;


    MagneticTrack* maganeticTrack;
    float agv_allowLineOffset;//unit is m　，目标点允许角度偏差
    float agv_allowAngleOffset;//unit is rad，目标点允许距离偏差
    float agv_allowLineMovingOffset;//unit is m，允许距离偏差
    float agv_allowLineMovingOffset_ratio;//判断倍率
    float agv_allowAngleMovingOffset;//unit is rad,允许的角度偏差
    double currentLineOffset;//unit is m,实时轨迹跟随误差
    double currentAngleOffset;//unit is rad,实时轨迹跟随误差，ｂｕｇ 直线段的时错误的。

    double timeAxisScale;//时间轴比例尺，内部求点时，需要修正v=vOLD/timeAxisScale, a=aOLD/timeAxisScale^2 .
    double axisAccelerationRatio;//加速度比例
    TimeAxisParameter timeAxisParameter;
    int timeAxisStatus;
    double oldTimeAxisRatio;
    double newTimeAxisRatio;
    AxisObject* timeAxis;//时间轴
    bool isServOn;
    double totalSpeedRatio;//safeSpeedRatio*controlSpeedRatio*pidSpeedRatio
    double safeSpeedRatio;//外部安全用调速
    double controlSpeedRatio;//手动速率，自动速率用。
    double pidSpeedRatio;//pid调节用。
    double adjustSpeedRatio;//cnt拐弯减速调节用。
    double adjustAngleOffset_startRatio;//开始偏差调节角度
    double adjustAngleOffset_endRatio;//结束偏差调节角度
    double adjustRatioForAngleOffset;//角度偏差对应的调节系数
    double adjustTimeForAngleOffset;//角度偏差对应的调节时间

    double adjustLineOffset_startRatio;//开始偏差调节角度
    double adjustLineOffset_endRatio;//结束偏差调节角度
    double adjustRatioForLineOffset;//角度偏差对应的调节系数
    double adjustTimeForLineOffset;//角度偏差对应的调节时间

    double noTransMoveDistance;//m, 距离小于该值不平移运动
    double noRotateMoveAngle;//rad,不旋转角度


    int updateTrajStage;
    double absoluteDistance_old;
    double absoluteAngle_old;
    int isUltrasonicValid;
    int isPointCloudCheckValid;

    LaserSafetyDetect* laserSafetyDetect;
    VirtualLineBias* virtualLineBias;


    double lineAcc_slowdown;
    double rotateAcc_slowdown;

    E_VEHICLE_TYPE vehicleType;
    E_PATH_CONTROL_STRATEGY    pathControlStrategy;

    int debugFlag;
    bool isLaserLose;
    RobotIo *ioController;
    volatile double barrierProtectRatio;//停止\暂停\没有运动轨迹时,为正常状态.运动轨迹时,正常检测

};

#endif // VIRTUALLINECONTROL_H
